/* support to GSL - GNU Scientific Library
 * * Parameters class
 * * Multiroot Solver - metod F
 * * Multiroot Solver - metod FdF
 */
#include"supToGSL.h"
using namespace std;

/* Parameters to Multiroot Solver */
int Parameters::set(unsigned int _n, double *_p)
{
	n = _n;
	p = _p;
	return 0;
}

int Parameters::setValue(double *_p)
{
	if(_p == NULL || p == NULL)
	{ fprintf(stderr, "pointer is void\n"); exit(EXIT_FAILURE); }
	for(unsigned int i=0; i<n; i++) { p[i] = _p[i]; }
	return 0;
}

double* Parameters::getPointer()
{
	if (p == NULL) { fprintf(stderr, "pointer is void\n"); }
	return p;
}

void Parameters::getCopy(double *x)
{
	if (p == NULL) { fprintf(stderr, "pointer is void\n"); x=NULL; }
	for(unsigned int i=0; i<n; i++) { x[i] = p[i]; }
}

unsigned int Parameters::dim() { return n; }

void Parameters::deleteP() { delete[] p; }

int Parameters::print()
{
	if(p == NULL) return -1;
	printf("parametrs:");
	for(unsigned int i=0; i<n; i++) { printf(" %11.3e", p[i]); }
	printf("\n");
	return 0;
}

double & Parameters::operator[](unsigned int el)
{
	if(p == NULL) { fprintf(stderr, "pointer is void\n"); exit(EXIT_FAILURE); }
	if(el >= n) { fprintf(stderr, "error scope"); exit(EXIT_FAILURE); }
	return p[el];
}

const double & Parameters::operator[](unsigned int el) const
{
	if(p == NULL) { fprintf(stderr, "pointer is void\n"); exit(EXIT_FAILURE); }
	if(el > n) { fprintf(stderr, "error scope"); exit(EXIT_FAILURE); }
	return p[el];
}

int Parameters::shake(unsigned int numberOfShake)
{
	double r = static_cast<double>(rand()%10000)/10000.;
	double d = static_cast<double>(numberOfShake*numberOfShake)/10;
	fprintf(stderr, "Shake %d: r = %.3f\n", numberOfShake, (r-0.5)*d);
	for(unsigned int i=0; i<n; i++)
	{
		// for really small p[i] shake procedure add somethich proporcional
		// to 10^{-5}, for bigger p[i] something proportional to p[i].
		bool sign = (p[i] > 0);
		p[i] += (p[i] + 0.00001*exp(-fabs(p[i])*100000))*(r-0.5)*d;
		if(sign != (p[i] > 0)) p[i] *= (-1);
		fprintf(stderr, "%.3e ", p[i]);
	}
	fprintf(stderr, "\n");

	return 0;
}

int Parameters::interpolation(double *xSet, double **ySet, unsigned int depth, unsigned int current, double xValue, double h)
{
	if(DEBUG) printf("current=%d, n=%d: ", current, n);
	if(current == 0)
	{
		for(unsigned int i=0; i<n; i++)
		{
			p[i] = ySet[i][0];
			if(DEBUG) printf("i: %.6f ", p[i]);
		}
		if(DEBUG) printf("\n");
		return 0;
	}

	if(current == 1)
	{
		for(unsigned int i=0; i<n; i++) { p[i] = 2*ySet[i][1]-ySet[i][0]; }
		return 0;
	}

	unsigned int N = current+1;
	if(N > depth) { N = depth; }
	double **y = new double*[n];
	double *x = new double[N];
	if(h>0)
	{
		for(unsigned int i=0; i<N; i++) { x[i] = xSet[current+1-N+i]; }
		for(unsigned int i=0; i<n; i++)
		{
			y[i] = new double[N];
			for(unsigned int j=0; j<N; j++) { y[i][j] = ySet[i][current+1-N+j]; }
		}
	}
	else
	{
		for(unsigned int i=0; i<N; i++) { x[N-i-1] = xSet[current+1-N+i]; }
		for(unsigned int i=0; i<n; i++)
		{
			y[i] = new double[N];
			for(unsigned int j=0; j<N; j++) { y[i][N-j-1] = ySet[i][current+1-N+j]; }
		}
	}
	xValue += h;

	gsl_interp_accel *acc = gsl_interp_accel_alloc ();
	gsl_spline *spline = gsl_spline_alloc (gsl_interp_cspline, N);

	if(DEBUG) printf("interpolation:");
	for(unsigned int i=0; i<n; i++)
	{
		gsl_spline_init (spline, x, y[i], N);
		p[i] = gsl_spline_eval (spline, xValue, acc);
		if(DEBUG) printf(" %.3e", p[i]);
	}
	if(DEBUG) printf("\n");

    gsl_spline_free (spline);
    gsl_interp_accel_free(acc);
	for(unsigned int i=0; i<n; i++) { delete []y[i]; }
	delete []y;
	delete []x;
	return 0;
}

/* Multiroot Solver */
MultirootSolver::MultirootSolver(unsigned int _n) : n(_n), status(-1)
{
	gslX0 = gsl_vector_alloc(_n);
}

MultirootSolver::~MultirootSolver()
{
	gsl_vector_free(gslX0);
}

/* Multiroot Solver - metod 1 - f*/
MultirootSolverF::MultirootSolverF(functionF _f, unsigned int _n) : MultirootSolver(_n), f(_f)
{
	gslT = gsl_multiroot_fsolver_hybrids;
	gslS = gsl_multiroot_fsolver_alloc(gslT, n);
}

MultirootSolverF::~MultirootSolverF()
{
	gsl_multiroot_fsolver_free(gslS);
}

int MultirootSolverF::print()
{
	printf("iter = %4d | x = ", (int)iter);
	for(unsigned int i=0; i<n; i++)
	{ printf("%10.3e ",gsl_vector_get(gslS->x,i)); }
	printf("| f(x) = ");
	for(unsigned int i=0; i<n; i++)
	{ printf("%10.3e ",gsl_vector_get(gslS->f,i)); }
	printf("\n");
	return 0;
}

int MultirootSolverF::printResult()
{
	printf("%s %d\t", gsl_strerror(status), (int)iter);
	for(unsigned int i=0; i<n; i++)
	{ printf("%.12e\t",gsl_vector_get(gslS->x,i)); }
	printf("\n");
	return 0;
}

int MultirootSolverF::printResult(FILE *fp)
{
	fprintf(fp, "%s %d\t", gsl_strerror(status), (int)iter);
	for(unsigned int i=0; i<n; i++)
	{ fprintf(fp, "%.12e\t",gsl_vector_get(gslS->x,i)); }
	fprintf(fp, "\n");
	fflush(fp);
	return 0;
}

int MultirootSolverF::solveFor(Parameters *Par, Parameters *X0)
{
	// Check if data is accurated
	if(X0->dim() != n)
	{
		fprintf(stderr,"MultirootSolver::solveFor, wrong number of equations");
		exit(EXIT_FAILURE);
	}

	// Clean previous iterations
	iter = 0;

	// ATTENTION: for next line you have to use c++0x standard
	// it's mean: you have to use -std=c++0x flag during compilation
	// gslF = {f, n, reinterpret_cast<void*>(Par)};
	// However one can do it in other way:
	gslF.f = f;
	gslF.n = n;
	gslF.params = reinterpret_cast<void*>(Par);

	for(unsigned int i=0; i<n; i++) { gsl_vector_set (gslX0, i, (*X0)[i]); }
	gsl_multiroot_fsolver_set (gslS, &gslF, gslX0);
	if(DEBUG) { print(); }
	do
	{
		iter++;
		status = gsl_multiroot_fsolver_iterate (gslS);
		if(DEBUG) { print(); }
		if(status) { break; }
		status = gsl_multiroot_test_residual (gslS->f, PRECISION);
	} while(status == GSL_CONTINUE && iter < MAX_ITERATIONS);
	if(DEBUG) { printf("status = %s\n", gsl_strerror(status)); }

	return 0;
}

Parameters* MultirootSolverF::getResult()
{
	if(status != GSL_SUCCESS)
	{ fprintf(stderr, "status != GSL_SUCCESS\n"); return NULL; }
	double *result = new double[n];
	for(unsigned int i=0; i<n; i++) { result[i] = gsl_vector_get(gslS->x,i); }
	Parameters *Result = new Parameters(n, result);
	//POD ZADNYM POZOREM NIE DAWAC TU "delete[] result;"
	return Result;
}

/* Multiroot Solver - metod 2 - fdf*/
MultirootSolverFDF::MultirootSolverFDF(functionF _f, functionDF _df, functionFDF _fdf, unsigned int _n) : MultirootSolver(_n), f(_f), df(_df), fdf(_fdf)
{
	gslT = gsl_multiroot_fdfsolver_gnewton;
	gslS = gsl_multiroot_fdfsolver_alloc(gslT, n);
}

MultirootSolverFDF::~MultirootSolverFDF()
{
	gsl_multiroot_fdfsolver_free(gslS);
}

int MultirootSolverFDF::print()
{
	printf("iter = %4d | x = ", (int)iter);
	for(unsigned int i=0; i<n; i++)
	{ printf("%11.3e ",gsl_vector_get(gslS->x,i)); }
	printf("| f(x) = ");
	for(unsigned int i=0; i<n; i++)
	{ printf("%11.3e ",gsl_vector_get(gslS->f,i)); }
	printf("\n");
	return 0;
}

int MultirootSolverFDF::solveFor(Parameters *Par, Parameters *X0)
{
	// Check if data is accurated
	if(X0->dim() != n)
	{
		fprintf(stderr,"MultirootSolver::solveFor, wrong number of equations");
		exit(EXIT_FAILURE);
	}

	// Clean previous iterations
	iter = 0;

	// ATTENTION: for next line you have to use c++0x standard
	// it's mean: you have to use -std=c++0x flag during compilation
	// gslFDF = {f, df, fdf, n, reinterpret_cast<void*>(Par)};
	// However one can do it in other way:
	gslFDF.f = f;
	gslFDF.df = df;
	gslFDF.fdf = fdf;
	gslFDF.n = n;
	gslFDF.params = reinterpret_cast<void*>(Par);

	for(unsigned int i=0; i<n; i++) { gsl_vector_set (gslX0, i, (*X0)[i]); }
	gsl_multiroot_fdfsolver_set (gslS, &gslFDF, gslX0);
	if(DEBUG) { print(); }
	do
	{
		iter++;
		status = gsl_multiroot_fdfsolver_iterate (gslS);
		if(DEBUG) { print(); };
		if(status) { break; }
		status = gsl_multiroot_test_residual (gslS->f, PRECISION);
	} while(status == GSL_CONTINUE && iter < MAX_ITERATIONS);
	if(DEBUG) { printf("status = %s\n", gsl_strerror(status)); }

	return 0;
}

Parameters* MultirootSolverFDF::getResult()
{
	double *result = new double[n];
	if(status != GSL_SUCCESS)
	{ fprintf(stderr, "status != GSL_SUCCESS\n"); return NULL; }
	for(unsigned int i=0; i<n; i++) { result[i] = gsl_vector_get(gslS->x,i); }
	Parameters *Result = new Parameters(n, result);
	delete[] result;
	return Result;
}
